/*******************************************************************************

  Pilot Intelligence Library
    http://www.pilotintelligence.com/

  ----------------------------------------------------------------------------

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program. If not, see <http://www.gnu.org/licenses/>.

*******************************************************************************/


#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include <algorithm>

#include "base/utils/utils.h"
#include "base/utils/utils_str.h"

#include "utils_mavlink.h"

using namespace std;

////////////////////////////////////////////////////////////////////////////////
/// enum to name
////////////////////////////////////////////////////////////////////////////////

char g_mavlink_autopilot_name[][200] =
{
    "Generic",
    "PIXHAWK",
    "Slugs",
    "APM",
    "OpenPilot",
    "Waypoints only",
    "Waypoints & Simple Nav",
    "Mission full",
    "INVALID",
    "PPZ",
    "UDB",
    "FP",
    "PX4",
    "SMACCMPILOT",
    "AUTOQUAD",
    "ARMAZILA",
    "AEROB",
    "ASLUAV",
    ""
};

int mavlink_autopilot_name(uint8_t ap, char **name)
{
    if ( ap > 17 ) {
        return -1;
    } else {
        *name = g_mavlink_autopilot_name[ap];
        return 0;
    }
}


////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

char g_mavlink_mav_type_name[][200] =
{
    "GENERIC",
    "FIXED_WING",
    "QUADROTOR",
    "COAXIAL",
    "HELICOPTER",
    "ANTENNA_TRACKER",
    "GCS",
    "AIRSHIP",
    "FREE_BALLOON",
    "ROCKET",
    "GROUND_ROVER",
    "SURFACE_BOAT",
    "SUBMARINE",
    "HEXAROTOR",
    "OCTOROTOR",
    "TRICOPTER",
    "FLAPPING_WING",
    "KITE",
    "ONBOARD_CONTROLLER",
    "VTOL_DUOROTOR",
    "VTOL_QUADROTOR",
    "GIMBAL",
    ""
};

int mavlink_mav_type_name(uint8_t mt, char **name)
{
    if( mt > 26 ) {
        return -1;
    } else {
        *name = g_mavlink_mav_type_name[mt];
        return 0;
    }
}



////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

char g_mavlink_mav_mode_name[][200] =
{
    "Custom",
    "Test",
    "Auto",
    "Guided",
    "Stabilize",
    "HIL",
    "Manual Input",
    "Armed",
    ""
};

int mavlink_mav_mode_getName(uint8_t mf, char **name)
{
    if( mf > 7 ) {
        return -1;
    } else {
        *name = g_mavlink_mav_mode_name[mf];
        return 0;
    }
}

int mavlink_mav_basemode_name(uint8_t mf, mavlink_nameList &nl)
{
    int         i;
    uint32_t    m;
    char        *name;

    m = 1;
    for(i=0; i<7; i++) {
        if( (mf & m) > 0 ) {
            mavlink_mav_mode_getName(i, &name);
            nl.push_back(name);
        }

        m = m << 1;
    }

    return 0;
}


////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

#define MAVLINK_FLIGHTMODE_NUM      17

enum g_mavlink_px4_custom_mainmode_name
{
    PX4_CUSTOM_MAIN_MODE_MANUAL = 1,
    PX4_CUSTOM_MAIN_MODE_ALTCTL,
    PX4_CUSTOM_MAIN_MODE_POSCTL,
    PX4_CUSTOM_MAIN_MODE_AUTO,
    PX4_CUSTOM_MAIN_MODE_ACRO,
    PX4_CUSTOM_MAIN_MODE_OFFBOARD,
    PX4_CUSTOM_MAIN_MODE_STABILIZED,
    PX4_CUSTOM_MAIN_MODE_RATTITUDE_LEGACY,
    PX4_CUSTOM_MAIN_MODE_SIMPLE /* unused, but reserved for future use */
};

enum g_mavlink_px4_custom_submode_auto_name
{
    PX4_CUSTOM_SUB_MODE_AUTO_READY = 1,
    PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF,
    PX4_CUSTOM_SUB_MODE_AUTO_LOITER,
    PX4_CUSTOM_SUB_MODE_AUTO_MISSION,
    PX4_CUSTOM_SUB_MODE_AUTO_RTL,
    PX4_CUSTOM_SUB_MODE_AUTO_LAND,
    PX4_CUSTOM_SUB_MODE_AUTO_RESERVED_DO_NOT_USE, // was PX4_CUSTOM_SUB_MODE_AUTO_RTGS, deleted 2020-03-05
    PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET,
    PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND
};

enum g_mavlink_px4_custom_submode_posctl_name
{
    PX4_CUSTOM_SUB_MODE_POSCTL_POSCTL = 0,
    PX4_CUSTOM_SUB_MODE_POSCTL_ORBIT
};

union g_mavlink_px4_custom_mode_int
{
    struct {
        uint16_t reserved;
        uint8_t main_mode;
        uint8_t sub_mode;
    };
    uint32_t data;
};

//custom mode for px4
//see here for more detail https://docs.px4.io/master/en/concept/flight_modes.html
char g_mavlink_px4_custommode_name[][256] =
{
    "MANUAL",
    "ALTCTL",
    "POSCTL",
    "AUTO_MISSION",
    "AUTO_LOITER",
    "AUTO_RTL",
    "AUTO_LANGENGFAIL",             //same as AUTO_LAND
    "ACRO",
    "DESCEND",                      //same as AUTO_LAND
    "TERMINATION",                  //same as MANUAL mode
    "OFFBOARD",
    "STABILIZED",
    "AUTO_TAKEOFF",
    "AUTO_LAND",
    "AUTO_FOLLOW_TARGET",
    "AUTO_PRECLAND",
    "ORBIT"
};

int mavlink_px4_custommode_getName(uint32_t cm, char** name, uint8_t& cmEnum)
{
    if( cm == uint32_t(PX4_CUSTOM_MAIN_MODE_MANUAL << 16) )
        cmEnum = 0;
    else if( cm == uint32_t(PX4_CUSTOM_MAIN_MODE_ALTCTL << 16) )
        cmEnum = 1;
    else if( cm == uint32_t(PX4_CUSTOM_MAIN_MODE_POSCTL << 16) )
        cmEnum = 2;
    else if( cm == uint32_t((PX4_CUSTOM_SUB_MODE_AUTO_MISSION << 24) | (PX4_CUSTOM_MAIN_MODE_AUTO << 16)) )
        cmEnum = 3;
    else if( cm == uint32_t((PX4_CUSTOM_SUB_MODE_AUTO_LOITER << 24) | (PX4_CUSTOM_MAIN_MODE_AUTO << 16)) )
        cmEnum = 4;
    else if( cm == uint32_t((PX4_CUSTOM_SUB_MODE_AUTO_RTL << 24) | (PX4_CUSTOM_MAIN_MODE_AUTO << 16)) )
        cmEnum = 5;
    else if( cm == uint32_t((PX4_CUSTOM_SUB_MODE_AUTO_LAND << 24) | (PX4_CUSTOM_MAIN_MODE_AUTO << 16)) )
        cmEnum = 6;
    else if( cm == uint32_t(PX4_CUSTOM_MAIN_MODE_ACRO << 16) )
        cmEnum = 7;
    else if( cm == uint32_t((PX4_CUSTOM_SUB_MODE_AUTO_LAND << 24) | (PX4_CUSTOM_MAIN_MODE_AUTO << 16)) )
        cmEnum = 8;
    else if( cm == uint32_t(PX4_CUSTOM_MAIN_MODE_MANUAL << 16) )
        cmEnum = 9;
    else if( cm == uint32_t(PX4_CUSTOM_MAIN_MODE_OFFBOARD << 16) )
        cmEnum = 10;
    else if( cm == uint32_t(PX4_CUSTOM_MAIN_MODE_STABILIZED << 16) )
        cmEnum = 11;
    else if( cm == uint32_t((PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF << 24) | (PX4_CUSTOM_MAIN_MODE_AUTO << 16)) )
        cmEnum = 12;
    else if( cm == uint32_t((PX4_CUSTOM_SUB_MODE_AUTO_LAND << 24) | (PX4_CUSTOM_MAIN_MODE_AUTO << 16)) )
        cmEnum = 13;
    else if( cm == uint32_t((PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET << 24) | (PX4_CUSTOM_MAIN_MODE_AUTO << 16)) )
        cmEnum = 14;
    else if( cm == uint32_t((PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND << 24) | (PX4_CUSTOM_MAIN_MODE_AUTO << 16)) )
        cmEnum = 15;
    else if( cm == uint32_t((PX4_CUSTOM_SUB_MODE_POSCTL_ORBIT << 24) | (PX4_CUSTOM_MAIN_MODE_POSCTL << 16)) )
        cmEnum = 16;

    *name = g_mavlink_px4_custommode_name[cmEnum];
    return 0;
}

uint32_t mavlink_px4_custommode_getInt(const char *cmName)
{
    g_mavlink_px4_custom_mode_int CMI;
    CMI.data = 0;
    if( !strcmp(cmName, "MANUAL" ) )
    {
        CMI.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
    }
    else if( !strcmp(cmName, "ALTCTL") )
    {
        CMI.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL;
    }
    else if( !strcmp(cmName, "POSCTL") )
    {
        CMI.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL;
    }
    else if( !strcmp(cmName, "AUTO_MISSION") )
    {
        CMI.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
        CMI.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
    }
    else if( !strcmp(cmName, "AUTO_LOITER") )
    {
        CMI.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
        CMI.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
    }
    else if( !strcmp(cmName, "AUTO_RTL") )
    {
        CMI.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
        CMI.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL;
    }
    else if( !strcmp(cmName, "AUTO_LANGENGFAIL") )
    {
        CMI.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
        CMI.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND;
    }
    else if( !strcmp(cmName, "ACRO") )
    {
        CMI.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO;
    }
    else if( !strcmp(cmName, "DESCEND") )
    {
        CMI.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
        CMI.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND;
    }
    else if( !strcmp(cmName, "TERMINATION") )
    {
        CMI.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
    }
    else if( !strcmp(cmName, "OFFBOARD") )
    {
        CMI.main_mode = PX4_CUSTOM_MAIN_MODE_OFFBOARD;
    }
    else if( !strcmp(cmName, "STABILIZED") )
    {
        CMI.main_mode = PX4_CUSTOM_MAIN_MODE_STABILIZED;
    }
    else if( !strcmp(cmName, "AUTO_TAKEOFF") )
    {
        CMI.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
        CMI.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF;
    }
    else if( !strcmp(cmName, "AUTO_LAND") )
    {
        CMI.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
        CMI.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND;
    }
    else if( !strcmp(cmName, "AUTO_FOLLOW_TARGET") )
    {
        CMI.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
        CMI.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET;
    }
    else if( !strcmp(cmName, "AUTO_PRECLAND") )
    {
        CMI.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
        CMI.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND;
    }
    else if( !strcmp(cmName, "ORBIT") )
    {
        CMI.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL;
        CMI.sub_mode = PX4_CUSTOM_SUB_MODE_POSCTL_ORBIT;
    }
    else
    {
        std::cerr << "[utils_mavlink]: px4 custom mode name not found." << std::endl;
        return 0;
    }

    return CMI.data;
}

int mavlink_px4_custommode_getID(const char* cmName)
{
    if( !strcmp(cmName, "MANUAL" ) )
    {
        return 0;
    }
    else if( !strcmp(cmName, "ALTCTL") )
    {
        return 1;
    }
    else if( !strcmp(cmName, "POSCTL") )
    {
        return 2;
    }
    else if( !strcmp(cmName, "AUTO_MISSION") )
    {
        return 3;
    }
    else if( !strcmp(cmName, "AUTO_LOITER") )
    {
        return 4;
    }
    else if( !strcmp(cmName, "AUTO_RTL") )
    {
        return 5;
    }
    else if( !strcmp(cmName, "AUTO_LANGENGFAIL") )
    {
        return 6;
    }
    else if( !strcmp(cmName, "ACRO") )
    {
        return 7;
    }
    else if( !strcmp(cmName, "DESCEND") )
    {
        return 8;
    }
    else if( !strcmp(cmName, "TERMINATION") )
    {
        return 9;
    }
    else if( !strcmp(cmName, "OFFBOARD") )
    {
        return 10;
    }
    else if( !strcmp(cmName, "STABILIZED") )
    {
        return 11;
    }
    else if( !strcmp(cmName, "AUTO_TAKEOFF") )
    {
        return 12;
    }
    else if( !strcmp(cmName, "AUTO_LAND") )
    {
        return 13;
    }
    else if( !strcmp(cmName, "AUTO_FOLLOW_TARGET") )
    {
        return 14;
    }
    else if( !strcmp(cmName, "AUTO_PRECLAND") )
    {
        return 15;
    }
    else if( !strcmp(cmName, "ORBIT") )
    {
        return 16;
    }
    else
    {
        std::cerr << "[utils_mavlink]: px4 custom mode name not found." << std::endl;
        return -1;
    }
}

////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

char g_mavlink_mav_state_name[][200] =
{
    "UNINIT",
    "BOOT",
    "CALIBRATING",
    "STANDBY",
    "ACTIVE",
    "CRITICAL",
    "EMERGENCY",
    "POWEROFF",
    ""
};

int mavlink_mav_state_name(uint8_t ms, char **name)
{
    if( ms > 7 ) {
        return -1;
    } else {
        *name = g_mavlink_mav_state_name[ms];
        return 0;
    }
}


////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

void mavlink_mav_cmdList(mavlink_nameList &nl)
{
    nl.clear();

    nl.push_back("Waypoint");
    nl.push_back("Takeoff");
    nl.push_back("Land");
}

std::string mavlink_mav_cmd2str(MAV_CMD cmd)
{
    if( cmd == MAV_CMD_NAV_WAYPOINT )       return "Waypoint";
    else if( cmd == MAV_CMD_NAV_TAKEOFF )   return "Takeoff";
    else if( cmd == MAV_CMD_NAV_LAND )      return "Land";

    return "Waypoint";
}

MAV_CMD mavlink_mav_str2cmd(const std::string &cmd)
{
    if( pi::trim(cmd) == "Waypoint" )       return MAV_CMD_NAV_WAYPOINT;
    else if( pi::trim(cmd) == "Home" )      return MAV_CMD_NAV_WAYPOINT;
    else if( pi::trim(cmd) == "Takeoff" )   return MAV_CMD_NAV_TAKEOFF;
    else if( pi::trim(cmd) == "Land" )      return MAV_CMD_NAV_LAND;

    return MAV_CMD_NAV_WAYPOINT;
}


////////////////////////////////////////////////////////////////////////////////
/// MAV_SYS_STATUS_SENSOR
////////////////////////////////////////////////////////////////////////////////

static MAVLINK_SYS_STATUS_SENSOR_IDNAME_Struct g_mavlink_sys_status_sensor_idname[] =
{
    MAV_SYS_STATUS_SENSOR_ID_NAME_DEF(MAV_SYS_STATUS_SENSOR_3D_GYRO,
                "3D Gyro", "3D Gryo"),
    MAV_SYS_STATUS_SENSOR_ID_NAME_DEF(MAV_SYS_STATUS_SENSOR_3D_ACCEL,
                "3D Accelerometer", "3D Gryo"),
    MAV_SYS_STATUS_SENSOR_ID_NAME_DEF(MAV_SYS_STATUS_SENSOR_3D_MAG,
                "3D Magnetometer", "3D Magnetometer"),
    MAV_SYS_STATUS_SENSOR_ID_NAME_DEF(MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE,
                "Differential pressure", "Differential pressure"),
    MAV_SYS_STATUS_SENSOR_ID_NAME_DEF(MAV_SYS_STATUS_SENSOR_GPS,
                "GPS", "GPS"),
    MAV_SYS_STATUS_SENSOR_ID_NAME_DEF(MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW,
                "Optical flow", "Optical flow"),
    MAV_SYS_STATUS_SENSOR_ID_NAME_DEF(MAV_SYS_STATUS_SENSOR_VISION_POSITION,
                "Computer vision position", "Computer vision position"),
    MAV_SYS_STATUS_SENSOR_ID_NAME_DEF(MAV_SYS_STATUS_SENSOR_LASER_POSITION,
                "laser based position", "laser based position"),
    MAV_SYS_STATUS_SENSOR_ID_NAME_DEF(MAV_SYS_STATUS_SENSOR_EXTERNAL_GROUND_TRUTH,
                "External ground truth", "External ground truth"),
    MAV_SYS_STATUS_SENSOR_ID_NAME_DEF(MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL,
                "3D angular rate control", "3D angular rate control"),
    MAV_SYS_STATUS_SENSOR_ID_NAME_DEF(MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION,
                "Attitude stabilization", "Attitude stabilization"),
    MAV_SYS_STATUS_SENSOR_ID_NAME_DEF(MAV_SYS_STATUS_SENSOR_YAW_POSITION,
                "Yaw position", "Yaw position"),
    MAV_SYS_STATUS_SENSOR_ID_NAME_DEF(MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL,
                "z/altitude control", "z/altitude control"),
    MAV_SYS_STATUS_SENSOR_ID_NAME_DEF(MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL,
                "x/y position control", "x/y position control"),
    MAV_SYS_STATUS_SENSOR_ID_NAME_DEF(MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS,
                "motor outputs / control", "motor outputs / control"),
    MAV_SYS_STATUS_SENSOR_ID_NAME_DEF(MAV_SYS_STATUS_SENSOR_RC_RECEIVER,
                "rc receiver", "rc receiver"),
    MAV_SYS_STATUS_SENSOR_ID_NAME_DEF(MAV_SYS_STATUS_SENSOR_3D_GYRO2,
                "2nd 3D gyro", "2nd 3D gyro"),
    MAV_SYS_STATUS_SENSOR_ID_NAME_DEF(MAV_SYS_STATUS_SENSOR_3D_ACCEL2,
                "2nd 3D accelerometer", "2nd 3D accelerometer"),
    MAV_SYS_STATUS_SENSOR_ID_NAME_DEF(MAV_SYS_STATUS_SENSOR_3D_MAG2,
                "2nd 3D magnetometer", "2nd 3D magnetometer"),
    MAV_SYS_STATUS_SENSOR_ID_NAME_DEF(MAV_SYS_STATUS_GEOFENCE,
                "geofence", "geofence"),
    MAV_SYS_STATUS_SENSOR_ID_NAME_DEF(MAV_SYS_STATUS_AHRS,
                "AHRS subsystem health", "AHRS subsystem health"),
    MAV_SYS_STATUS_SENSOR_ID_NAME_DEF(MAV_SYS_STATUS_TERRAIN,
                "z/altitude control", "z/altitude control"),

    MAV_SYS_STATUS_SENSOR_ID_NAME_DEF(MAV_SYS_STATUS_SENSOR_ENUM_END,
                "None", "None"),
};

int mavlink_sys_status_sensor_getIDs(uint32_t s,
                                        mavlink_sys_status_sensor_list &l)
{
    int         i;
    uint32_t    m;

    m = 1;

    l.clear();

    for(i=0; i<31; i++) {
        if( (s & m) != 0 ) {
            l.push_back(m);
        }

        m = m << 1;
    }

    return 0;
}


int mavlink_sys_status_sensor_getName(uint32_t id, char **name)
{
    int     i;

    i = 0;
    *name = NULL;

    while(1) {
        if( g_mavlink_sys_status_sensor_idname[i].id == MAV_SYS_STATUS_SENSOR_ENUM_END ) {
            break;
        }

        if( g_mavlink_sys_status_sensor_idname[i].id == id ) {
            *name = g_mavlink_sys_status_sensor_idname[i].name;
            return 0;
        }

        i++;
    }

    return -1;
}

int mavlink_sys_status_sensor_getNames(mavlink_sys_status_sensor_list &l,
                                       mavlink_sys_status_sensor_namelist &nl)
{
    char    *name;
    mavlink_sys_status_sensor_list::iterator it;

    nl.clear();

    for(it=l.begin(); it!=l.end(); it++) {
        if( 0 == mavlink_sys_status_sensor_getName(*it, &name) ) {
            nl.push_back(name);
        } else {
            break;
        }
    }

    return 0;
}

int mavlink_sys_status_sensor_getID_Difference(mavlink_sys_status_sensor_list &l1,
                                               mavlink_sys_status_sensor_list &l2,
                                               mavlink_sys_status_sensor_list &ld)
{
    mavlink_sys_status_sensor_list  _l1, _l2;
    mavlink_sys_status_sensor_list::iterator it;

    mavlink_sys_status_sensor_list   v(32);

    _l1 = l1;
    _l2 = l2;

    std::sort(_l1.begin(), _l1.end());
    std::sort(_l2.begin(), _l2.end());

    it = std::set_difference(_l1.begin(), _l1.end(), _l2.begin(), _l2.end(), v.begin());
    v.resize(it-v.begin());
    ld = v;

    return 0;
}



////////////////////////////////////////////////////////////////////////////////
/// Data stream ID functions
////////////////////////////////////////////////////////////////////////////////

/// FIXME: need sync to mavlink definitions
std::vector<int> mavlink_getStreamIDs(void)
{
    std::vector<int> lst;

    lst.push_back(MAV_DATA_STREAM_RAW_SENSORS);
    lst.push_back(MAV_DATA_STREAM_EXTENDED_STATUS);
    lst.push_back(MAV_DATA_STREAM_RC_CHANNELS);
    lst.push_back(MAV_DATA_STREAM_RAW_CONTROLLER);
    lst.push_back(MAV_DATA_STREAM_POSITION);
    lst.push_back(MAV_DATA_STREAM_EXTRA1);
    lst.push_back(MAV_DATA_STREAM_EXTRA2);
    lst.push_back(MAV_DATA_STREAM_EXTRA3);

    return lst;
}
